#ifndef _MOYING_MAP_H_
#define _MOYING_MAP_H_

#include <moying_costmap/moying_costmap.h>
#include <utilities/utilities.h>
#include <deque>
#include <boost/dynamic_bitset.hpp>
#include <dispatch_system/definitions.h>
#include <dispatch_system/vehicle.h>
#include <thread>
#include <mutex>
#include <dispatch_system/diff_drive_vehicle.h>
#include <dispatch_system/mecanum_vehicle.h>
#include <dispatch_system/custom_movepatterns.h>
#include <dispatch_system/one_way_road.h>

namespace moying
{
namespace navigation
{

    typedef std::deque<MapObjectPtr> TimeOccupationList;
    typedef std::shared_ptr<TimeOccupationList> TimeOccupationListPtr;

    typedef std::vector<std::vector<TimeOccupationListPtr>> TimedMap;
    typedef std::shared_ptr<TimedMap> TimedMapPtr;

    typedef std::vector<MapCell> OccupiedSpace;
    typedef std::shared_ptr<OccupiedSpace> OccupiedSpacePtr;

    using ErgodicRobotCallback = std::function<bool(VehiclePtr vehicle)>;
    using CorrectCells = std::vector<std::vector<CorrectCellInfo>>;

    //匹配不同车辆尺寸的cell偏移点
    struct CorrectCellInSize{
        CorrectCells cells;     //偏移过导航点的cell
        double  vehicle_width;  //和cell匹配的车辆宽度
        double  vehicle_length; //和cell匹配的车辆长度
        CustomMovePatternsPtr custom_move_patterns; //每个cell的有效运动模式
    };
    
    class MoyingMap
    {
        public:
            /**
             * @brief Default constructor
             */
            MoyingMap();

            /**
             * @brief Default deconstructor
             */
            virtual ~MoyingMap();

            bool init_success(){return init_success_;}
            void set_init_success(bool value){init_success_ = value;}
            
            /**
             * @brief initialize
             */
            bool init(const CostMapPtr &costmap);

            /**
             * @brief initialize timed_map
             */
            bool initializeTimedMap(double T, double dt);

            /**
             * @brief get costmap
             */
            CostMapPtr getCostMap();

            /**
             * @brief get timed_map
             */
            TimedMapPtr getTimedMap();
            
            float time_resolution(){return time_resolution_;}

            PathPlanParam path_plan_param(){return path_plan_param_;}

            /**
             * @brief at state, there is no vehicle and no inflation
             */
            // bool isFree(const VehiclePtr &vehicle, const NodePtr &state);


            /**
             * @brief from state a to b, there is no vehicle and no inflation
             */
            bool isFree(const VehiclePtr &vehicle, const NodePtr &sa, const NodePtr &sb);

            //bool isPassable(const VehiclePtr &vehicle, const NodePtr &sa, const NodePtr &sb);

            /**
             * @brief at state, occupy timed_map
             */
            bool occupy(const VehiclePtr &vehicle, const Node &state);
            bool occupy(const VehiclePtr &vehicle, const NodePtr &state);
            bool occupy(const VehiclePtr &vehicle, int x, int y, int t);

            /**
             * @brief from sa to sb, occupy timed_map
             */
            bool occupy(const VehiclePtr &vehicle, const Node &sa, const Node &sb, std::map<int, std::vector<FootprintCell>> &occupied_ceels);
            bool occupy(const VehiclePtr &vehicle, const NodePtr &sa, const NodePtr &sb, std::map<int, std::vector<FootprintCell>> &occupied_ceels);
            bool occupy(const VehiclePtr &vehicle, int xa, int ya, int ta, int sb, int yb, int tb, std::map<int, std::vector<FootprintCell>> &occupied_ceels);
            

            void occupyTimeMap(const VehiclePtr vehicle,PlanningResult &result);

            /**
             * @brief occupyTimeMap 在机器人当前位置从当前时刻占用一定时间的时间地图
             * @param   robot_id   机器人id
             * @param   occupy_seconds  占用的秒数
             * @return  返回成功或失败
             */
            bool occupyTimeMap(unsigned int robot_id,unsigned int occupy_seconds);

            // inline int toIndex(int x, int y, int th, int t)
            // {
            //     return x + size_x_ * y + size_x_ * size_y_ * th + size_x_ * size_y_ * size_th_ * t;
            // }

            inline int toIndex(int x, int y, int th, int t,int sm=0)
            {
                if(size_sm_ > 0)
                    return x + size_x_ * y + size_x_ * size_y_ * th + size_x_ * size_y_ * size_th_ * sm + size_x_ * size_y_ * size_th_ * size_sm_* t;
                else
                    return x + size_x_ * y + size_x_ * size_y_ * th + size_x_ * size_y_ * size_th_ * t;

            }

            // void fromIndex(int index,int& x, int& y, int& th, int& t);

            void fromIndex(int index,int& x, int& y, int& th, int& t, int& sm);

            inline int getTimeCounts()
            {
                return time_counts_;
            }

            bool clearVehicleFootprint(const VehiclePtr &vehicle, TimedFootprints &footprint);
            bool clearVehiclePath(VehiclePtr &vehicle);
            bool clearVehiclePath(unsigned int id);

            bool isFree(const OccupiedSpace &occup);
            bool registerConstantOccupySpace(const std::string &name, const OccupiedSpace &occup);

            // bool registerStaticRobotOccupation(const VehiclePtr &vehicle);
            // bool registerStaticRobotOccupation(unsigned int id);
            // bool unregisterStaticRobotOccupation(const VehiclePtr &vehicle);
            // bool unregisterStaticRobotOccupation(unsigned int id);

            std::chrono::system_clock::time_point getNextPlanningTime();

            bool hasVehicle(unsigned int id);
            bool addVehicle(unsigned int id, std::string type, const std::string &name , 
                    moying_proto::NavBaseType nav_base_type=moying_proto::NavBaseType::kOmniBaseType);
            bool deleteVehicle(unsigned int id);

            VehiclePtr getVehicle(unsigned int id);
            bool isOnMap(int x, int y, int th);
            bool isOnMap(int x, int y, int th, int t);
            void updateTimeMap(const std::chrono::system_clock::time_point &current_time,int update_times=1);
            void addTimeList(int x, int y);
            // void updateLocalObstacle(unsigned int id, const std::map<int, std::pair<int, int>> &obs, const std::map<int, std::pair<int, int>> &old);

            //update robot pose and footprint info
            bool updateVehiclePose(unsigned int id, double x, double y, double th);

            // wheather vehicle can be there(x, y ,t) in timed_map
            bool isTimeValid(const VehiclePtr &vehicle, int x, int y, int t);

            bool isAvaiableElimateDiff(const VehiclePtr &vehicle, PlanningResult& result, int time_delay_s, int begin_index);

            // correct timed_map_ and PlanningResult
            bool correctTimeMap(const VehiclePtr &vehicle, PlanningResult& result, int time_delay_s, int begin_index, std::vector<Node>& new_node_path);

            // empty vehicle occupy at time t
            bool empty(const VehiclePtr &vehicle, const Node &st, std::vector<FootprintCell> &footprint_cells);

            // empty range[t1, t2]  vehicle occupy
            bool empty(const VehiclePtr &vehicle, const Node &sa, const Node &sb, std::vector<FootprintCell> &footprint_cells);

            // wheather passable in range[t1, t2]
            bool isPassable(const VehiclePtr &vehicle, const Node &sa, const Node &sb, std::map<int, FootprintCell> &footprint_cells);
            bool isPassable(const VehiclePtr &vehicle, const NodePtr &sa, const NodePtr &sb, std::map<int, FootprintCell> &footprint_cells);

            // wheather passable at time t
            bool isPassable(const VehiclePtr &vehicle, short t, std::map<int, FootprintCell> &footprint_cells);
            
            // get FootprintCell
            bool getFootprintCell(const VehiclePtr &vehicle, std::map<int, FootprintCell> & footprint_cells);
            bool getFootprintCell(const VehiclePtr &vehicle, const Node &st, std::map<int, FootprintCell> & footprint_cells);
            bool getFootprintCell(const VehiclePtr &vehicle, const NodePtr &st, std::map<int, FootprintCell> & footprint_cells);

            // get FootprintCell
            bool getFootprintCell(const VehiclePtr &vehicle, const Node &sa, const Node &sb, std::map<int, FootprintCell> &footprint_cells);
            bool getFootprintCell(const VehiclePtr &vehicle, const NodePtr &sa, const NodePtr &sb, std::map<int, FootprintCell> & footprint_cells);

            std::chrono::system_clock::time_point current_time(){return current_time_;}
            //update robot local costmap data
            // bool updateLocalCostmap(unsigned int robot_id,std::shared_ptr<moying_proto::LocalCostMap> local_costmap_ptr);
            // bool updateLocalObstacle(unsigned int robot_id,std::shared_ptr<moying_proto::LocalObstacle> local_obstacle_ptr);
            bool updateLocalObstacle(unsigned int robot_id,const LocalObstacleInfo& obs_info);

            bool getLocalObstacle(unsigned int robot_id,LocalObstacleInfo& obs_info);

            //获取离机器人边缘最近的障碍物距离
            bool getRobotMinDisToObstacle(unsigned int robot_id,float& min_dis );

            /**
             * @brief 获取和指定机器人距离最近的机器人
             * @param [IN] vehicle     目标机器人 
             * @param [OUT] min_dis    最小距离
             * @return  和目标机器人距离最近的机器人,或者为nullptr
             */
            VehiclePtr getMinDisVehicle(VehiclePtr vehicle, float& min_dis );

            /**
             * @brief 检测机器人在指定的cell是否会与局部障碍物所占的cell碰撞 
             * @param [in] robot_id     机器人id 
             * @param [in] center_mx        矩形中心点世界坐标系x坐标
             * @param [in] center_my        矩形中心点世界坐标系y坐标
             * @param [in] check_robot_occupancy  true:检测机器人在该cell时机器人所占的所有cell是否与障碍物碰撞,false:只检测参数指定的cell是否碰撞 
             * @return  true:会发生相撞,false:不会发生相撞
             */ 
            bool checkCollideLocalObstacleCell(unsigned int robot_id,int center_mx,int center_my,bool check_robot_occupancy=false);


            /**
             * @brief 检测矩形框在指定的朝向下是否会与周围环境相撞
             * @param [in] length           矩形长度,单位米
             * @param [in] width            矩形宽度,单位米
             * @param [in] center_x         矩形中心x坐标
             * @param [in] center_y         矩形中心y坐标
             * @param [in] theta            矩形朝向,单位弧度
             * @param [in] vehicle          机器人,如果该参数不为nullptr,还要检测矩形是否会和机器人的局部障碍物发生碰撞
             * @return  true:会发生相撞,false:不会发生相撞
             */ 
            bool checkCollision(float length,float width,float center_x,float center_y,float theta,VehiclePtr vehicle=nullptr);

            bool checkCollision(float length,float width,float center_x,float center_y,float theta,Vehicle* vehicle);

            // //检测当前路径规划中的局部碰撞信息,当前的规划机器人和障碍物信息已保存
            bool checkCurrentPathPlanLocalCollision(int center_mx,int center_my);

            inline bool isPointInRect(Coordinate pt,Coordinate lt,Coordinate rt,Coordinate rb,Coordinate lb,int& time);

            MapDimension getDimensions();

            //getters
            const TimedMap* timed_map(){return &timed_map_;}
            const std::vector<std::vector<MapObjectPtr>>& robots_occupation(){return robots_occupation_;}
            // const CorrectCells* corrected_cells(){return &corrected_cells_;}
            const CorrectCells* corrected_cells(double length,double width);
            const CorrectCells* corrected_cells(VehiclePtr vehicle);
            const CorrectCells* corrected_cells(unsigned int vehicle_id);

            
            CustomMovePatternsPtr get_custom_move_patterns(VehiclePtr vehicle);

            //settters
            bool set_path_planing_vehicle_id(unsigned int vehicle_id);

        // private:
            // bool registerToAddonMap(const MapObjectPtr &v, const OccupiedSpacePtr &space);
            // bool unregisterToAddonMap(const MapObjectPtr &v, const OccupiedSpacePtr &space);
            void updateRobotPoseToTimedMap(const VehiclePtr &vehicle);

            bool getCellNavCoor(int mx, int my, double& wx, double& wy,unsigned int vehicle_id);

            bool getCellNavCoor(int mx, int my, double& wx, double& wy,VehiclePtr vehicle);

            /**
             * @brief 判断cell是否可以旋转到指定角度 
             * @param [in] mx       矩形长度,单位米
             * @param [in] my       矩形宽度,单位米
             * @param [in] theta1   旋转的起始朝向 
             * @param [in] theta2   旋转的目标朝向
             * @param [in] length   机器人长度
             * @param [in] width    机器人宽度
             * @return  
             */
            bool isCellCanRotate(int mx,int my,int theta1,int theta2,double length,double width);

             /**
             * @brief 检测矩形是否会与局部障碍物相撞
             * @param [in] robot_id     机器人id
             * @param [in] center_wx        矩形中心点世界坐标系x坐标
             * @param [in] center_wy        矩形中心点世界坐标系y坐标
             * @param [in] theta            矩形朝向
             * @param [OUT] pt_vector_ptr   输出检测的多边形在世界坐标系下的顶点坐标
             * @return  true:会发生相撞,false:不会发生相撞
             */ 
            bool checkCollideLocalObstacle(unsigned int robot_id,float center_wx,float center_wy,float theta=0
                    ,std::vector<Coordinate>* pt_vector_ptr=nullptr,float l_padding=0,
                    float w_padding=0,bool temp_obstacle=false,bool check_collide_with_static_map=false);

            /**
             * @brief 检测正方形边缘是否在障碍物上
             * @param [in] center_wx  中心点世界x坐标    
             * @param [in] center_wy  中心点世界y坐标    
             * @param [in] edge_len  边长，米  
             * @return  true:至少有一边在障碍物上，false：所有边都不在障碍物上
             */ 
            bool checkSquareEdgeIsObstacle(float center_wx,float center_wy,float edge_len);

            /**
             * @brief 安全遍历机器人
             * @param [in] cb     机器人遍历回调函数，回调函数返回true继续遍历，返回false时停止遍历 
             * @return  
             */ 
            void ergodicRobots(ErgodicRobotCallback cb);

            //获取所有机器人列表
            std::map<unsigned int, VehiclePtr> vehicles(){return vehicles_;}


            std::vector<OneWayRoadPtr>* all_one_way_road(){return &all_one_way_road_;}

            /**
             * @brief 判断足迹在最近的时间段是否被占用
             * @param   footprint  机器人足迹
             * @param   seconds 从当前时刻开始的秒数
             * @param   except_robot_id    该机器人的足迹不认为占用
             *     
             * @return  true:指定cell在
             */ 
            bool isFootprintFreeInRecentSeconds(const FootprintOnMap& footprint,int seconds,int except_robot_id=0);


            /**
             * @brief 判断机器在指定点的足迹在最近的时间段是否被占用
             * @param   vehicle_id  机器人id
             * @param   mx  世界坐标
             * @param   my  世界坐标
             * @param   seconds 从当前时刻开始的秒数
             *     
             * @return  true:指定cell在最近的时间段不会被占用
             */ 
            bool isVehicleFreeLocateCellInRecentSeconds(unsigned int vehilce_id,double mx,double my,int seconds);

            /**
             * @brief 判断坐标是否在单向道内
             * @param   [IN] x                              x坐标
             * @param   [IN] y                              y坐标
             * @param   [OUT] road_id           单向道id
             * @param   [OUT] vehicle_ids   单向道中的机器人id
             *     
             * @return  true:在单向道内，false:不在单向道内
             */
            bool isPoseInOneWayRoad(float x,float y,unsigned int& road_id,std::vector<unsigned int>& vehicle_ids);

        private:
            //寻找并构建单向道对象
            void findOneWayRoad();

        private:
            bool init_success_ = false; //是否初始化成功
            int size_x_;    //路径规划用地图宽度
            int size_y_;    //路径规划用地图高度
            int size_th_;
            int size_sm_;
            int time_counts_;
            float time_resolution_; //时间分辨率,一个时刻代表多少秒
            unsigned int path_planing_vehicle_id_;  //正在路径规划的机器人id
            VehiclePtr path_planing_vehicle_ptr_;       //正在路径规划的机器人对象指针
            LocalObstacleInfo path_planing_obs_info_;//正在路径规划的机器人局部障碍物信息
            std::recursive_mutex vehicle_mutex_; 
            // std::map<unsigned int,LocalObstacleInfo > local_obstacle_; //局部障碍物信息
            // CorrectCells corrected_cells_;//更改过导航点坐标的cell信息
            std::vector<CorrectCellInSize>  corrected_cells_in_size_;//不同尺寸机器人更改过导航点的cell容器
            // CustomMovePatternsPtr custom_move_patterns_=nullptr;

            std::recursive_mutex map_mutex_;
            TimedMap timed_map_;
            TimedMapPtr timed_map_ptr_;
            CostMapPtr costmap_;
            std::chrono::system_clock::time_point current_time_;
            std::map<unsigned int, VehiclePtr> vehicles_;   //所有机器人容器,key:机器人id,value:机器人对象指针
            std::map<unsigned int, FootprintOnMap> vehicle_footprints_; //所有机器人足迹容器,key:机器人id,value:机器人足迹容器,配合更新robots_occupation_
            std::vector<std::vector<MapObjectPtr>> robots_occupation_;  //存储地图上的机器人占用信息,rviz显示用到
            std::vector<OneWayRoadPtr> all_one_way_road_;  //地图中所有单向道
            PathPlanParam path_plan_param_; //路径规划参数 
    };
    
} // namespace navigation
} // namespace moying

#endif // _MOYING_MAP_H_
